/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include "main.h"
#include "StochCB.h"
#include "conv_cor.h"
#include "scb_code.h"
#include "stoch.h"

STATIC void CalcStochGain(
fxpt_16 LPCImpResp[SF_LEN],
fxpt_16 Residual[RES_LEN],
int     *cbindex,
fxpt_32 *StochGain,
int     *codeword);

/**************************************************************************
*                                                                         *
* ROUTINE
*               StochasticAnalysis
*
* FUNCTION
*               Find optimal MSPE excitation code word
* SYNOPSIS
*               StochasticAnalysis(scale, LPCImpResp, Residual,
*                       StochasticIndex, StochasticGain, OptExc)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       scale           fxpt_16  i      Mixed excitation scaling factor
*       LPCImpResp      fxpt_16  i      LPC Impulse response
*       Residual        fxpt_16  i      Residual from adaptive analysis
*       StochasticIndex int      o      Stochastic codebook index
*       StochasticGain  int      o      Stochastic codebook gain
*       OptExc          fxpt_16  o      Optimum excitation vector
*
**************************************************************************
*
* DESCRIPTION
*
*  Code book search is performed by closed-loop analysis using conventional
*  minimum squared prediction error (MSPE) criteria of the perceptually
*  weighted error signal.  The code book is overlaped to allow recursive
*  computational savings in routine ConvCor:
*
*       index   code book
*               +-----------------------+
*       1       | 2M-1         2(M-1)+L |
*       2       | 2(M-2)+1     2(M-2)+L |
*       :       | :            :        |
*       N-1     | .            .        |
*       N       | .            .        |
*       :       | 3            62       |
*       :       | 1            60       |
*               +-----------------------+
*
*       where: M = maximum code book size
*              N = actual code book search size (any value between 1 & M)
*              L = code word length
*
*       each code word is:  2(M-index)+1 -> 2(M-index)+L
*
***************************************************************************/
void StochasticAnalysis(
fxpt_16 scale,                  /* 3.12 format */
fxpt_16 LPCImpResp[SF_LEN],     /* 2.13 format */
fxpt_16 Residual[RES_LEN],      /* 15.0 format */
int     *StochasticIndex,
fxpt_16 *StochasticGain,        /* 11.4 format */
fxpt_16 OptExc[SF_LEN])         /* 15.0 format */
{
        fxpt_32 gain;
        int     i;
        int     gindex;
        int     codeword;

FXPT_PUSHSTATE("StochasticAnalysis", -1.0, -1.0);
        /* Calculate gain for each codeword, search for best codeword */
        CalcStochGain(LPCImpResp, Residual, StochasticIndex, &gain,
            &codeword);

        /* Apply constrained excitation factor */
        /**StochasticGain *= scale;*/
        *StochasticGain = fxpt_saturate16(
            fxpt_mult64_round_good(gain, fxpt_deposit_l(scale), 12));
        // *StochasticGain = fxpt_mult16_round(*StochasticGain, scale, 12);

        /* Quantize gain */
        *StochasticGain = SCBGainEncode(fxpt_saturate16(gain), &gindex);

        /*  Scale selected code word vector into excitation array */
        for (i=0; i<SF_LEN; i++)
                /*OptExc[i] = *StochasticGain*StochCB[i+codeword];*/
                OptExc[i] = fxpt_mult16_round(*StochasticGain,
                    StochCB[i+codeword], 4);
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               CalcStochGain
*
* FUNCTION
*               Find gain and -error term for each codeword
*               and search for best code word (max -error term)
*               (codewords are overlapped by shifts of -2
*               along the code vector x)
*               NOTE:  gain[i] and err[i] are replaced by scalars
* SYNOPSIS
*               CalcStochGain(LPCImpResp, Residual, cbindex, gain, codeword)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       LPCImpResp      fxpt_16  i      LPC Impulse Response
*       Residual        fxpt_16  i      Residual after adaptive analysis
*       cbindex         int      o      Pointer to best codeword
*       StochGain       int      o      Best gain
*       codeword        int      o      Best codeword
*
**************************************************************************/


void CalcStochGain(
fxpt_16 LPCImpResp[SF_LEN],             /* 2.13 format */
fxpt_16 Residual[RES_LEN],              /* 15.0 format */
int     *cbindex,
fxpt_32 *StochGain,                     /* 27.4 format */
int     *codeword)
{
        fxpt_32 err, emax;              /* 30.1 format */
        int     i;
        fxpt_32 gain;                   /* 27.4 format */

FXPT_PUSHSTATE("CalcStochGain", -1.0, -1.0);
        /* Initialize */
        *codeword = 2*MAX_STOCH_CB_SIZE-2;
        *cbindex = 1;
        *StochGain = ConvCor(&StochCB[*codeword], TRUE, LPCImpResp,
                             Residual, &err);
        emax = err;
        *codeword -= 2;

        /* Calculate gain and error */
        for (i=1; i<STOCH_CB_SIZE; i++) {
                gain = ConvCor(&StochCB[*codeword], FALSE, LPCImpResp,
                               Residual, &err);
                *codeword -= 2;
                if (err >= emax) {
                        emax = err;
                        *cbindex = i+1;
                        *StochGain = gain;
                }
        }

        /* Calculate pointer to best code word */
        *codeword = 2*(MAX_STOCH_CB_SIZE - *cbindex);
FXPT_POPSTATE();
}
